import serial

from math import sqrt, ceil, hypot
import os
import os.path
import time



DEVICE_DIR = "/dev"

SERIAL_BAUDRATE = 9600
SERIAL_NOP_TIMEOUT = 0.5
SERIAL_CMD_TIMEOUT = 3

ARM_RIGHT = 1
ARM_LEFT = 2
ROT_UP = 4
ROT_DOWN = 8
PEN_UP = 16
PEN_DOWN = 32

EGGBOT_OK = '0'

arm_edge = 260
scale_factor = 1



class EggBot:
  def __init__(self):
    self.port = None
    self.arm = self.arm_act = 0.0
    self.spin = self.spin_act = 0.0

  def move(self, arm, spin):
    if arm == 0 and spin == 0:
      return

    arm, spin = scale_factor*arm, scale_factor*spin

    # These represent the commands we might send to the Arduino, if we move enough.
    am = ARM_RIGHT if arm > 0 else 0 if arm == 0 else ARM_LEFT
    rt = ROT_UP if spin > 0 else 0 if spin == 0 else ROT_DOWN

    r = max(1, hypot(arm, spin))
    a0 = self.arm
    s0 = self.spin

    # Draw a straight line from our current position to the new position.
    arm_end = self.arm + arm
    arm_increment = arm / r
    spin_end = self.spin + spin
    spin_increment = spin / r
    for i in range(1, ceil(r) + 1):
      arm_next = a0 + (min(i*arm_increment, arm) if arm >= 0 else max(i*arm_increment, arm))
      spin_next = s0 + (min(i*spin_increment, spin) if spin >= 0 else max(i*spin_increment, spin))
      #print self.arm, self.spin, self.arm_act, self.spin_act, arm_next, spin_next

      cmd = 0
      if self.arm_act != round(arm_next) and abs(round(arm_next)) <= arm_edge:
        cmd = cmd | am
      if self.spin_act != round(spin_next):
        cmd = cmd | rt

      if not self.__send(cmd): break

      self.arm_act, self.spin_act = round(arm_next), round(spin_next)
      self.arm, self.spin = arm_next, spin_next

  def move_absolute(self, arm, spin):
    # Use self.move to move us to the absolute position.
    self.move(arm - self.arm, spin - self.spin)

  def pen_up(self):
    self.__send(PEN_UP)

  def pen_down(self):
    self.__send(PEN_DOWN)

  def __close(self):
    if self.port:
      self.port.close()
      self.port = None

  def __check(self, ping=False):
    if not self.port:
      return False

    try:
      # First read off anything that might already be in the buffer.
      while self.port.inWaiting():
        if self.port.read() != EGGBOT_OK:
          self.__close()
          return False

      if ping:
        self.port.timeout = SERIAL_NOP_TIMEOUT
        self.port.write(chr(0))
        result = self.port.read()
      else:
        # I think this will force an exception if something bad is going on.
        self.port.timeout = self.port.timeout
        result = EGGBOT_OK
    except serial.SerialException:
      result = -1 # Benign error message.
    except IOError:
      result = -1
    except OSError:
      result = -1

    if result == EGGBOT_OK:
      return True
    else:
      if result:
        self.__close()
      return False

  def __try_to_connect(self):
    # For now use a NOP to test if the port really works.  This is actually a terrible way to do this
    # because there is no guarantee that it won't fail later.
    if not self.__check():
      for device in filter(lambda name: name.startswith("ttyUSB"), os.listdir(DEVICE_DIR)):
        device = os.path.join(DEVICE_DIR, device)
        try:
          self.port = serial.Serial(device, SERIAL_BAUDRATE)

          # The Arduino takes a bit to get into the main program.  Poll for 3 seconds before timing out.
          for i in range(30):
            time.sleep(0.1)
            if self.__check(ping=True): break # Connected.
            if not self.port: continue

        except serial.SerialException:
          self.__close()

      # If we just connected set the assumed starting position.
      if self.port:
        self.arm = self.arm_act = 0.0
        self.spin = self.spin_act = 0.0

  def __send(self, cmd):
    self.__try_to_connect()
    if self.port:
      try:
        self.port.timeout = SERIAL_CMD_TIMEOUT
        self.port.write(chr(cmd))
        return self.port.read() == EGGBOT_OK
      except serial.SerialException:
        self.__close()
      except IOError:
        self.__close()
      except OSError:
        self.__close()
    return False
